package fem2.element.crack_sda;

import inf.jlinalg.Array1DMatrix;
import inf.jlinalg.Array2DMatrix;
import inf.jlinalg.ArrayVector;
import inf.jlinalg.IMatrix;
import inf.jlinalg.IVector;
import inf.math.LinAlg;

import java.util.ArrayList;

import math2.LinAlgUtilities;
import math2.MathUtilities;
import fem2.MaterialModel;
import fem2.MeshPart;
import fem2.enu.MeshPartType;
import fem2.enu.StressType;
import fem2.jlinalg.RotationMatrix;
import fem2.material.cohesive_law.CohesiveLaw;

public class SOSConstantJumpTetrahedraElement extends ConstantJumpSDAElement {

	protected static final double sqrt_1_3 = Math.sqrt(1.0 / 3);

	protected SeparationMode bestMode = null;

	public SOSConstantJumpTetrahedraElement(MeshPart mp, MaterialModel mm, CohesiveLaw cl) {
		super(mp, mm, cl);
		setCrackMeshPartType(MeshPartType.TRI3);
		setIntegrationOrderOnCrack(1);
	}

	public SOSConstantJumpTetrahedraElement(MeshPart mp, MaterialModel mm, CohesiveLaw cl,
			StressType stressType) {
		this(mp, mm, cl);
		this.stressType = stressType;
	}

	@Override
	protected void computeEnhancedStrainOperator(MeshPart mp, IMatrix g, double... xi) {
		double coeff = -crackMeasure / elementMeasure;
		g.set(0, 0, coeff * normal[0]);
		g.set(1, 1, coeff * normal[1]);
		g.set(2, 2, coeff * normal[2]);

		g.set(3, 0, coeff * normal[1]);
		g.set(3, 1, coeff * normal[0]);

		g.set(4, 1, coeff * normal[2]);
		g.set(4, 2, coeff * normal[1]);

		g.set(5, 0, coeff * normal[2]);
		g.set(5, 2, coeff * normal[0]);
	}

	@Override
	protected void computeProjectionOperator(MeshPart mp, IMatrix g, double... xi) {
		computeEnhancedStrainOperator(mp, g, xi);
	}

	// protected int posNode;

	// @Override
	// protected boolean checkOnsetCriteria() {
	// IVector sigma = getStress(0, stressType);
	// MeshPart mp = getMeshPart();
	// int n = mp.getBasis().getDimension();
	// int dim = mp.getDimension();
	// int ntrs = dim * (dim + 1) / 2;
	// /*
	// * perform spectral decomposition
	// */
	// IVector ps = new ArrayVector(dim);
	// IMatrix pd = new Array2DMatrix(dim, dim);
	// LinAlgUtilities.decomposePrincipalStress(dim, sigma, ps, pd);
	//
	// if (ps.get(2) < getCohesiveLaw().getTensileStrength()) {
	// return false;
	// } else {
	//
	// /*
	// * realize crack normal
	// */
	// double n1 = pd.get(0, 2);
	// double n2 = pd.get(1, 2);
	// double n3 = pd.get(2, 2);
	//
	// /*
	// * compute B operator
	// */
	// Array1DMatrix B = Array1DMatrix.createColumnMajor(ntrs, dim * n);
	// double[] xi = getGaussPoint(0);
	// computeB(mp, B, xi);
	//
	// /*
	// * search the gradient which make smallest angle with crack normal
	// * and align the crack with that gradient (strictly enforcing
	// * orthogonal condition)
	// */
	// double cos = 0.0;
	// for (int i = 0; i < 4; i++) {
	// double gradx = B.get(0, i * dim);
	// double grady = B.get(1, 1 + i * dim);
	// double gradz = B.get(2, 2 + i * dim);
	// double normgrad = Math.sqrt(gradx * gradx + grady * grady + gradz *
	// gradz);
	// gradx /= normgrad;
	// grady /= normgrad;
	// gradz /= normgrad;
	// double tmp = Math.abs(n1 * gradx + n2 * grady + n3 * gradz);
	// if (tmp > cos) {
	// cos = tmp;
	// posNode = i;
	// normal[0] = gradx;
	// normal[1] = grady;
	// normal[2] = gradz;
	// /*
	// * compute crack area
	// */
	// crackArea = elementVolume * normgrad;
	// }
	// }
	//
	// /*
	// * find the normal of the surface (n1,n2,n3) , (gradx,grady,gradz)
	// */
	// double[] z = LinAlgUtilities.cross(n1, n2, n3, normal[0], normal[1],
	// normal[2]);
	// double normz = LinAlg.norm2(dim, z);
	//
	// if (normz > LinAlgUtilities.tol14) {
	// /*
	// * find the angle between (n1,n2,n3) and (gradx,grady,gradz)
	// */
	// cos = n1 * normal[0] + n2 * normal[1] + n3 * normal[2];
	// double theta = Math.acos(cos);
	// /*
	// * rotate other two principal stresses
	// */
	// RotationMatrix rot = new RotationMatrix(0, 0, 0, z[0], z[1], z[2],
	// theta);
	// tangent1 = rot.timesXYZ(pd.get(0, 0), pd.get(1, 0), pd.get(2, 0));
	// tangent2 = rot.timesXYZ(pd.get(0, 1), pd.get(1, 1), pd.get(2, 1));
	// } else {
	// tangent1 = new double[] { pd.get(0, 0), pd.get(1, 0), pd.get(2, 0) };
	// tangent2 = new double[] { pd.get(0, 1), pd.get(1, 1), pd.get(2, 1) };
	// }
	//
	// /*
	// * compute crack boundary
	// */
	// int[] negNodes;
	// if (posNode == 0) {
	// negNodes = new int[] { 1, 2, 3 };
	// } else if (posNode == 1) {
	// negNodes = new int[] { 0, 2, 3 };
	// } else if (posNode == 2) {
	// negNodes = new int[] { 0, 1, 3 };
	// } else if (posNode == 3) {
	// negNodes = new int[] { 0, 1, 2 };
	// } else {
	// throw new Error("positive node is wrong");
	// }
	//
	// Node[] nodes = mp.getNodes();
	// double[] X1 = new double[dim];
	// double[] X2 = new double[dim];
	// double[] X3 = new double[dim];
	// double[] posNodePos = nodes[posNode].getPosition();
	// double[] negNode1Pos = nodes[negNodes[0]].getPosition();
	// double[] negNode2Pos = nodes[negNodes[1]].getPosition();
	// double[] negNode3Pos = nodes[negNodes[2]].getPosition();
	//
	// X1[0] = posNodePos[0] * (1 - sqrt_1_3) + negNode1Pos[0] * sqrt_1_3;
	// X1[1] = posNodePos[1] * (1 - sqrt_1_3) + negNode1Pos[1] * sqrt_1_3;
	// X1[2] = posNodePos[2] * (1 - sqrt_1_3) + negNode1Pos[2] * sqrt_1_3;
	//
	// X2[0] = posNodePos[0] * (1 - sqrt_1_3) + negNode2Pos[0] * sqrt_1_3;
	// X2[1] = posNodePos[1] * (1 - sqrt_1_3) + negNode2Pos[1] * sqrt_1_3;
	// X2[2] = posNodePos[2] * (1 - sqrt_1_3) + negNode2Pos[2] * sqrt_1_3;
	//
	// X3[0] = posNodePos[0] * (1 - sqrt_1_3) + negNode3Pos[0] * sqrt_1_3;
	// X3[1] = posNodePos[1] * (1 - sqrt_1_3) + negNode3Pos[1] * sqrt_1_3;
	// X3[2] = posNodePos[2] * (1 - sqrt_1_3) + negNode3Pos[2] * sqrt_1_3;
	//
	// crackBoundary.add(X1);
	// crackBoundary.add(X2);
	// crackBoundary.add(X3);
	//
	// if (getEchoLevel().contain(EchoLevelType.AUX4)) {
	// Debugger.watch("sigma = ", sigma);
	// Debugger.watch("crack area = ", crackArea);
	// Debugger.watch("crack normal = ", normal);
	// Debugger.watch("element volume = ", elementVolume);
	//
	// // double[] cross = LinAlgUtilities.cross(X2[0] - X1[0], X2[1] -
	// // X1[1], X2[2] - X1[2],
	// // X3[0] - X1[0], X3[1] - X1[1], X3[2] - X1[2]);
	// // double tmpArea = 0.5 * LinAlg.norm2(dim, cross);
	// // Debugger.watch("crack area 2 = ", tmpArea);
	//
	// }
	//
	// return true;
	// }
	// }

	@Override
	protected boolean checkOnsetCriteria() {
		IVector sigma = getStress(0, stressType);
		MeshPart mp = getMeshPart();
		int n = mp.getBasis().getDimension();
		int dim = mp.getDimension();
		int ntrs = dim * (dim + 1) / 2;
		/*
		 * perform spectral decomposition
		 */
		IVector ps = new ArrayVector(dim);
		IMatrix pd = new Array2DMatrix(dim, dim);
		LinAlgUtilities.decomposePrincipalStress(dim, sigma, ps, pd);

		if (ps.get(2) < getCohesiveLaw().getTensileStrength()) {
			return false;
		} else {

			alpha = new ArrayVector(dim);

			/*
			 * compute B operator
			 */
			IMatrix B = Array1DMatrix.createColumnMajor(ntrs, dim * n);
			double[] xi = getIntegrationPoint(0);
			computeB(mp, B, xi);

			/*
			 * check and filter all valid separation modes
			 */
			ArrayList<SeparationMode> validModes = new ArrayList<SeparationMode>();
			// int[][] configurations = new int[][] { { 0 }, { 1 }, { 2 }, { 3
			// }, { 0, 1 }, { 0, 2 },
			// { 0, 3 }, { 1, 2 }, { 1, 3 }, { 2, 3 } };
			int[][] configurations = new int[][] { { 0 }, { 1 }, { 2 }, { 3 }, { 0, 1 }, { 0, 2 },
					{ 0, 3 } };

			for (int i = 0; i < configurations.length; i++) {
				double gradx = 0, grady = 0, gradz = 0;
				for (int j = 0; j < configurations[i].length; j++) {
					gradx += B.get(0, configurations[i][j] * dim);
					grady += B.get(1, 1 + configurations[i][j] * dim);
					gradz += B.get(2, 2 + configurations[i][j] * dim);
				}
				double normgrad = Math.sqrt(gradx * gradx + grady * grady + gradz * gradz);
				gradx /= normgrad;
				grady /= normgrad;
				gradz /= normgrad;
				/*
				 * compute crack area
				 */
				double A = elementMeasure * normgrad;
				/*
				 * check the separation mode
				 */
				SeparationMode mode = new SeparationMode(mp, new ArrayVector(gradx, grady, gradz),
						A, configurations[i]);
				if (mode.validate()) {
					validModes.add(mode);
				}
			}

			/*
			 * realize crack normal
			 */
			double n1 = pd.get(0, 2);
			double n2 = pd.get(1, 2);
			double n3 = pd.get(2, 2);

			/*
			 * find all in valid separation modes, the mode make the smallest
			 * angle with crack normal vector from Rankine criterion
			 */
			double cos = 0.0;
			for (SeparationMode mode : validModes) {
				IVector N = mode.getSeparationNormal();
				double tmp = N.get(0) * n1 + N.get(1) * n2 + N.get(2) * n3;
				if (Math.abs(tmp) > Math.abs(cos)) {
					cos = tmp;
					bestMode = mode;
				}
			}

			/*
			 * get the normal from the best mode to become normal of the crack
			 */
			normal = ((ArrayVector) bestMode.getSeparationNormal()).getValues();
			crackMeasure = bestMode.getCrackArea();

			/*
			 * rotate the local crack frame obtain from Rankine criterion to the
			 * frame of best mode (strictly enforcing orthogonalization)
			 */
			double[] z = MathUtilities.cross(n1, n2, n3, normal[0], normal[1], normal[2]);
			double normz = LinAlg.norm2(dim, z);

			if (normz > MathUtilities.tol14) {
				/*
				 * find the angle between (n1,n2,n3) and (gradx,grady,gradz)
				 */
				cos = n1 * normal[0] + n2 * normal[1] + n3 * normal[2];
				double theta = Math.acos(cos);
				/*
				 * rotate other two principal stresses
				 */
				RotationMatrix rot = new RotationMatrix(0, 0, 0, z[0], z[1], z[2], theta);
				tangent1 = rot.timesXYZ(pd.get(0, 0), pd.get(1, 0), pd.get(2, 0));
				tangent2 = rot.timesXYZ(pd.get(0, 1), pd.get(1, 1), pd.get(2, 1));
			} else {
				tangent1 = new double[] { pd.get(0, 0), pd.get(1, 0), pd.get(2, 0) };
				tangent2 = new double[] { pd.get(0, 1), pd.get(1, 1), pd.get(2, 1) };
			}

			/*
			 * compute crack boundary
			 */
			crackBoundary = bestMode.getCrackBoundary();

			return true;
		}
	}

	// private class Pair implements Comparable<Pair> {
	//
	// private double s;
	// private Node n;
	//
	// public Pair(double s, Node n) {
	// this.s = s;
	// this.n = n;
	// }
	//
	// /**
	// * @return the s
	// */
	// public double getProjectionCoordinate() {
	// return s;
	// }
	//
	// /**
	// * @return the n
	// */
	// public Node getNode() {
	// return n;
	// }
	//
	// @Override
	// public int compareTo(Pair o) {
	// if (s < o.s) {
	// return -1;
	// } else if (s > o.s) {
	// return 1;
	// } else {
	// return 0;
	// }
	// }
	//
	// }

}
